
package JET.physics.engine;

import JET.geom.Vec2d;
import JET.physics.Mass;
import JET.physics.MaterialPair;
import JET.physics.PhysicsHelper;
import JET.physics.SBCollision;
import JET.physics.Wall;
import JET.physics.bodies.RigidBody;
import java.util.Collection;

/**
 *
 * @author raos
 */
public class RBCollider {

    private static final SBCollision infoTouch = new SBCollision();
    private static final SBCollision infoPenet = new SBCollision();
    private static final SBCollision infoAway = new SBCollision();
    private static final SBCollision infoSame = new SBCollision();
    private static final Vec2d hitPt = new Vec2d();
    private static final Vec2d norm = new Vec2d();
    private static final Vec2d bVel = new Vec2d();
    private static final Vec2d relVel = new Vec2d();
    private static final Vec2d helpVecA = new Vec2d();
    private static final Vec2d helpVecB = new Vec2d();
    private static final Vec2d numV = new Vec2d();
    private static final Vec2d tangent = new Vec2d();
    private static final double[] edgeD = new double[1];
    private static final Vec2d tempVec = new Vec2d();

    public static double mPenetrationThreshold = 250;

    public static void collisionCheck_RigidBodies(JETContext c) {
        RigidBody   rb1 = null,
                    rb2 = null;

        for (int i=0; i<c.rigidBodyList.size()-1; i++)
            for (int j=i+1; j<c.rigidBodyList.size(); j++)
            {
                rb1 = c.rigidBodyList.get(i);
                rb2 = c.rigidBodyList.get(j);

                MaterialPair matP = c.materials[rb1.getMaterial()][rb2.getMaterial()];
                if (matP.collide) {
                    collide(c, rb1, rb2 );
                    collide(c, rb2, rb1 );
                }
            }
    }

    public static void collide(JETContext c, RigidBody rbA, RigidBody rbB) {

//        // Check touching
//        for(int i=0; i<rbA.border.getWallsCount(); i++) {
//            Mass mass = rbA.border.getWall(i).getM1();
//
//            infoTouch.clear();
//            infoTouch.body1 = rbA;
//            infoTouch.bodyApm = mass;
//            infoTouch.body2 = rbB;
//
//            double minDist = Double.MAX_VALUE;
//            for (int j=0; j<rbB.border.getWallsCount(); j++)
//            {
//                Wall wall = rbB.border.getWall(j);
//
//                // test against this edge.
//                double dist = rbB.border.getClosestPointOnEdgeSquared(mass.getPosition(), wall, hitPt, norm, edgeD);
//
//                Vec2d vA = new Vec2d(mass.getPosition());
//                rbA.sceneToBase(vA);
//                vA.makePerp();
//                vA.scale(rbA.omega);
//                vA.rotate(rbA.angle);
//                vA.add(rbA.velocity);
//
//                Vec2d vB = new Vec2d(hitPt);
//                rbB.sceneToBase(vB);
//                vB.makePerp();
//                vB.scale(rbB.omega);
//                vB.rotate(rbB.angle);
//                vB.add(rbB.velocity);
//
//                Vec2d vAB = new Vec2d(vA.getSubbed(vB));
//                if (vAB.dot(norm) >= 0) continue;
//
//                if (dist<minDist) {
//                    minDist = dist;
//                    infoTouch.bodyBwall = wall;
//                    infoTouch.edgeD = edgeD[0];
//                    infoTouch.hitPt.become(hitPt);
//                    infoTouch.norm.become(norm);
//                    infoTouch.penetration = dist;
//                    infoTouch.vA.become(vA);
//                    infoTouch.vB.become(vB);
//                    infoTouch.vAB.become(vAB);
//                }
//            }
//
//            if (minDist<=1)
//            {
//                infoTouch.penetration = -Math.sqrt(Math.abs(infoTouch.penetration));
//                c.rbColInfoList.get(c.rbColInfoCount++).set(infoTouch);
//
//                if (c.rbColInfoCount>=c.rbColInfoList.size())
//                    for(int abcd=0; abcd<50; abcd++)
//                        c.rbColInfoList.add(new SBCollision());
//            }
//        }

        // Check penetration
        for(int i=0; i<rbA.border.getWallsCount(); i++ ) {
            Mass mass = rbA.border.getWall(i).getM1();

            if ( !rbB.getBoundingArea().contains(mass.getPosition()) )
                continue;

            if ( !rbB.border.contains( mass.getPosition()) )
                continue;

            Vec2d ptNorm = rbA.border.getMassNormal(mass);

            double closestAway = 1000000.;

            infoAway.clear();
            infoAway.body1 = rbA;
            infoAway.bodyApm = mass;
            infoAway.body2 = rbB;

            hitPt.become(Vec2d.zero);
            norm.become(Vec2d.zero);

            edgeD[0] = 0;
            for (int j=0; j<rbB.border.getWallsCount(); j++) {
                Wall wall = rbB.border.getWall(j);

                // test against this edge.
                double dist = rbB.border.getClosestPointOnEdgeSquared(mass.getPosition(), wall, hitPt, norm, edgeD);

                double dot = ptNorm.dot(norm);
                if (dot <= 0. && dist < closestAway) {
                    closestAway = dist;
                    infoAway.bodyBwall = wall;
                    infoAway.edgeD = edgeD[0];
                    infoAway.hitPt.become(hitPt);
                    infoAway.norm.become(norm);
                    infoAway.penetration = dist;
                }
                else continue;
            }

            Vec2d vA = new Vec2d(mass.getPosition());
            rbA.sceneToBase(vA);
            vA.makePerp();
            vA.scale(rbA.omega);
            vA.rotate(rbA.angle);
            vA.add(rbA.velocity);

            hitPt.become(mass.getPosition());
            Vec2d vB = new Vec2d(hitPt);
            rbB.sceneToBase(vB);
            vB.makePerp();
            vB.scale(rbB.omega);
            vB.rotate(rbB.angle);
            vB.add(rbB.velocity);

            Vec2d vAB = new Vec2d(vA.getSubbed(vB));

            if (vAB.dot(infoAway.norm) >= 0) continue;

            infoAway.vA.become(vA);
            infoAway.vB.become(vB);
            infoAway.vAB.become(vAB);
            infoAway.penetration = Math.sqrt(infoAway.penetration);
            c.rbColInfoList.get(c.rbColInfoCount++).set(infoAway);

            if (c.rbColInfoCount>=c.rbColInfoList.size())
                for(int abcd=0; abcd<50; abcd++)
                    c.rbColInfoList.add(new SBCollision());
        }
    }

    public static void handleCollisions(JETContext c) {
        double e = 0;
        for(int i=0; i<c.rbColInfoCount; i++) {
            SBCollision col = c.rbColInfoList.get(i);
            RigidBody rbA = (RigidBody) col.body1;
            RigidBody rbB = (RigidBody) col.body2;

            double vABmag = col.vAB.getMagnitude();
            if (vABmag>=.01)
            {
                // Translation
                double j = col.vAB.getScaled(-(1+e)).dot(col.norm) / col.norm.dot( col.norm.getScaled(1/rbA.mass + 1/rbB.mass) );

                rbA.velocity.add( col.norm.getScaled(j/rbA.mass));
                rbB.velocity.sub( col.norm.getScaled(j/rbB.mass));

                //Rotation
                Vec2d rAPp = new Vec2d(col.bodyApm.getPosition());
                rAPp.sub(PhysicsHelper.getMassCenterOfMasses(rbA.dynVecList));
                rAPp.makePerp();
                Vec2d rBPp = new Vec2d(col.hitPt);
                rBPp.sub(PhysicsHelper.getMassCenterOfMasses(rbB.dynVecList));
                rBPp.makePerp();

                j = col.vAB.getScaled(-(1+e)).dot(col.norm) /
                        ( col.norm.dot(col.norm.getScaled(1/rbA.mass + 1/rbB.mass)) + Math.pow(rAPp.dot(col.norm), 2)/rbA.mI +  Math.pow(rBPp.dot(col.norm), 2)/rbB.mI );

                rbA.omega += rAPp.dot( col.norm.getScaled(j) )/rbA.mI;
                rbB.omega -= rBPp.dot( col.norm.getScaled(j) )/rbB.mI;
                //System.out.println("ped zmieniony... vAB: "+ vABmag);
            }

            if (col.penetration>0) {
                Vec2d move = col.norm.getScaled(col.penetration*.5);

                rbA.separationV.add(move);
                rbA.separationV.scale(.25);
                rbB.separationV.sub(move);
                rbB.separationV.scale(.25);
            }
        }
    }

    public static void zeroSeparationVecs(JETContext c) {
        Collection<RigidBody> rbCol = c.getRigidBodyListSync();
        synchronized(rbCol) {
            for(RigidBody rb : rbCol)
                rb.separationV.zero();
        }
    }
}